/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Watchdog;

/**
 *
 * Disabled Mode:
 * - Once per second, print (on the console) the number of seconds the robot has been disabled.
 *
 * Autonomous Mode:
 * - Follow line.
 * - Hang Tube.
 *
 * Teleop Mode:
 * - Duh.
 *
 * This code assumes the following connections:
 * - Driver Station:
 *   - USB 1 - The right joystick.  Used for either "arcade drive" or "right" stick for tank drive
 *   - USB 2 - The left joystick.  Used as the "left" stick for tank drive
 *   - USB 3 - The copilot joystick.
 *
 * - Robot:
 *   - Digital Sidecar 1:
 *     - PWM 1 - Connected to left drive motor(s)
 *     - PWM 2 - Connected to right drive motor(s)
 *
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class DefaultRobot extends IterativeRobot {
   
    /** robot will use PWM 1&2 for drive motors */
    private RobotDrive robotDrive;                    
    
    private DigitalInput pressureValve;

    private DriveStationFF driveStation;

    // By convention left = 1, right = 2, and copilot = 3
    private Joystick joyRight;			
    private Joystick joyLeft;
    private Joystick coPilot;

    /** Joystick array for iterating over the joysticks
     * 1 = left
     * 2 = right
     * 3 = coPilot */
    private Joystick[] joyArray = new Joystick[4];
    
    private Relay compressor;
    
   // Methods for dealing with the arm
    private RobotArmFF arm;

    private final AutonomousFF autonomous;
    
    //Used to print Disabled seconds to the console.
    private static int printSec;
    private static int startSec;

    private Solenoid deployment;
    private Solenoid superShyphter;
    private Solenoid  deployAssist;

    /*=================== Constructor ======================================= */
    
    public DefaultRobot() {
        System.out.println("BuiltinDefaultCode Constructor Started\n");

         
        // Robot drive uses PWM channels 1 and 2
        robotDrive = new RobotDrive(1,2);
        
        // Preasurevalve. 
        pressureValve = new DigitalInput(14);
        compressor = new Relay(1);

        // Joysticks
        joyRight = new Joystick(1);
        joyLeft = new Joystick(2);
        coPilot = new Joystick(3);
        
        joyArray[1] = joyLeft;
        joyArray[2] = joyRight;
        joyArray[3] = coPilot;

        // arm
        arm = new RobotArmFF(joyArray);

        // initizes the drive station;
        driveStation = new DriveStationFF();

        // autonomous 
        autonomous = new AutonomousFF(robotDrive, arm, driveStation);

        superShyphter = new Solenoid(4);

        deployment = new Solenoid(8);

        deployAssist = new Solenoid(2);

        System.out.println("BuiltinDefaultCode Constructor Completed\n");
    }

    /* ================ Init Methods ======================================= */
    public void robotInit() {
        // Actions which would be performed once (and only once) upon 
        // initialization of the robot would be put here.
        
        
        
        driveStation.dSPrint("Welcome Team 2194!!");
        driveStation.dSPrint("Code vs. 3-12-11 #3");

        
        System.out.println("RobotInit() completed.\n");
    }

    public void disabledInit() {
     
        startSec = (int) (Timer.getUsClock() / 1000000.0);
        printSec = startSec + 1;
       
    }

    public void autonomousInit() {
        compressor.set(Relay.Value.kForward);
        autonomous.init();
        
    }

    public void teleopInit() {
        compressor.set(Relay.Value.kForward);
    }

    /* =================== Periodic Routines =========================== */
    
    public void disabledPeriodic() {
        // feed the user watchdog at every period when disabled
        Watchdog.getInstance().feed();

        // while disabled, printout the duration of current disabled mode in seconds
        if ((Timer.getUsClock() / 1000000.0) > printSec) {
            System.out.println("Disabled seconds: " + (printSec - startSec));
            printSec++;
            
        }
    }

    public void autonomousPeriodic() {
        // Let the arena know we are still here.
        Watchdog.getInstance().feed();
        
        MaintainCompressor();
        autonomous.update();
    }

    public void teleopPeriodic() {
        // Let the arena know we are still here.
        Watchdog.getInstance().feed();                  

        MaintainCompressor();

        updateSolenoids();

        arm.update();
       
        robotDrive.tankDrive(softStart(joyLeft.getRawAxis(2)),softStart(joyRight.getRawAxis(2)));

        driveStation.update();
    }

    /* =============== Helper Methods ======================================= */

    private void updateSolenoids(){
        superShyphter.set(joyArray[1].getRawButton(1) || joyArray[2].getRawButton(1));
        deployment.set(joyArray[1].getRawButton(8));
        deployAssist.set(joyArray[3].getRawButton(1));
    }

    private void MaintainCompressor() {
       if (pressureValve.get()) {
            compressor.set(Relay.Value.kOff);
        } else {
            compressor.set(Relay.Value.kForward);
        }
    }

    /** Softens joysticks 
     * @param x 
     * @return signOf(x) * (x^2) */
    private double softStart(double x){ return ((x > 0)? 1 : -1)* (x * x); }

}